import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

class Obstacle:
    def __init__(self, position, radius, obstacle_type):
        self.position = position
        self.radius = radius
        self.type = obstacle_type

class Target:
    def __init__(self, position, velocity):
        self.position = position
        self.velocity = velocity

class AutonomousAircraft:
    def __init__(self, position, velocity):
        self.position = position.astype(float)
        self.velocity = velocity.astype(float)

def get_perceptions(position, obstacles, targets):
    obstacle_detections = np.zeros(len(obstacles))
    for i, obstacle in enumerate(obstacles):
        distance = np.linalg.norm(obstacle.position - position)
        if distance < obstacle.radius:
            obstacle_detections[i] = 1
    target_detections = np.random.rand(len(targets))
    return obstacle_detections, target_detections

def avoid_obstacles(obstacle_detections, aircraft, obstacles):
    avoidance_direction = np.zeros(3)
    for i, obstacle_detected in enumerate(obstacle_detections):
        if obstacle_detected:
            obstacle_position = obstacles[i].position
            avoidance_direction += aircraft.position - obstacle_position
    if np.linalg.norm(avoidance_direction) > 0:
        avoidance_direction /= np.linalg.norm(avoidance_direction)
    return avoidance_direction

def make_decision(obstacle_detections, target_detections, aircraft, targets, obstacles):
    avoidance_direction = avoid_obstacles(obstacle_detections, aircraft, obstacles)
    distances_to_targets = [np.linalg.norm(target.position - aircraft.position) for target in targets]
    nearest_target_index = np.argmin(distances_to_targets)
    nearest_target_position = targets[nearest_target_index].position
    direction_to_target = nearest_target_position - aircraft.position
    new_direction = direction_to_target / np.linalg.norm(direction_to_target)
    combined_direction = new_direction + avoidance_direction
    speed = 5.0
    new_velocity = speed * combined_direction
    return new_velocity, combined_direction

obstacles = [
    Obstacle(np.array([100, 100, 50]), 50, 'building'),
    Obstacle(np.array([200, 200, 100]), 50, 'airflow'),
    Obstacle(np.array([300, 300, 150]), 50, 'gravity')
]

targets = [
    Target(np.array([400, 400, 100]), np.array([10, 10, 10])),
    Target(np.array([500, 500, 100]), np.array([-10, -10, -10]))
]

initial_velocity = np.array([1, 0, 0])
aircraft = AutonomousAircraft(np.array([0, 0, 0]), initial_velocity)

dt = 0.5
total_time = 500

positions = []
velocities = []

t = 0
while t < total_time:
    obstacle_detections, target_detections = get_perceptions(aircraft.position, obstacles, targets)
    new_velocity, new_direction = make_decision(obstacle_detections, target_detections, aircraft, targets, obstacles)
    aircraft.position += aircraft.velocity * dt
    aircraft.velocity = new_velocity
    positions.append(aircraft.position.copy())
    velocities.append(aircraft.velocity)
    t += dt

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
for obstacle in obstacles:
    if obstacle.type == 'building':
        color = 'r'
        explanation = '红色圆球表示建筑物'
    elif obstacle.type == 'airflow':
        color = 'g'
        explanation = '绿色圆球表示空气流'
    else:
        color = 'c'
        explanation = '青色圆球表示重力区域'
    ax.scatter(obstacle.position[0], obstacle.position[1], obstacle.position[2], c=color, marker='o')
    print(explanation)
for target in targets:
    ax.scatter(target.position[0], target.position[1], target.position[2], c='b', marker='o')
    print('蓝色球体表示移动目标')
ax.plot([p[0] for p in positions], [p[1] for p in positions], [p[2] for p in positions], c='b') 
ax.quiver([p[0] for p in positions], [p[1] for p in positions], [p[2] for p in positions], 
          [v[0] for v in velocities], [v[1] for v in velocities], [v[2] for v in velocities], color='m')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title('Autonomous Aircraft Simulation')
plt.show()
